
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       jitter_correction.c
  * @author     baiyang
  * @date       2022-1-29
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "jitter_correction.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// constructor
// uint16_t max_lag_ms=500, uint16_t convergence_loops=100
void jitter_correction(JitterCorrection* jc, uint16_t max_lag_ms, uint16_t convergence_loops)
{
    jc->max_lag_ms = max_lag_ms;
    jc->convergence_loops = convergence_loops;
}

// correct an offboard timestamp to a jitter-free local
// timestamp. See JitterCorrection.cpp for details
uint64_t jitter_correct_offboard_timestamp_usec(JitterCorrection* jc, uint64_t offboard_usec, uint64_t local_usec)
{
    int64_t diff_us = (int64_t)(local_usec) - (int64_t)(offboard_usec);

    if (!jc->initialised ||
        diff_us < jc->link_offset_usec) {
        // this message arrived from the remote system with a
        // timestamp that would imply the message was from the
        // future. We know that isn't possible, so we adjust down the
        // correction value
        jc->link_offset_usec = diff_us;
        jc->initialised = true;
    }

    int64_t estimate_us = offboard_usec + jc->link_offset_usec;

    if (estimate_us + (jc->max_lag_ms*1000U) < (int64_t)(local_usec)) {
        // this implies the message came from too far in the past. clamp the lag estimate
        // to assume the message had maximum lag
        estimate_us = local_usec - (jc->max_lag_ms*1000U);
        jc->link_offset_usec = estimate_us - offboard_usec;
    }

    if (jc->min_sample_counter == 0) {
        jc->min_sample_us = diff_us;
    }
    jc->min_sample_counter++;
    if (diff_us < jc->min_sample_us) {
        jc->min_sample_us = diff_us;
    }
    if (jc->min_sample_counter == jc->convergence_loops) {
        // we have the requested number of samples of the transport
        // lag for convergence. To account for long term clock drift
        // we set the diff we will use in future to this value
        jc->link_offset_usec = jc->min_sample_us;
        jc->min_sample_counter = 0;
    }
    
    return (uint64_t)(estimate_us);

}

// correct an offboard timestamp to a jitter-free local
// timestamp. See JitterCorrection.cpp for details
uint32_t jitter_correct_offboard_timestamp_msec(JitterCorrection* jc, uint32_t offboard_ms, uint32_t local_ms)
{
    return jitter_correct_offboard_timestamp_usec(jc, offboard_ms*1000ULL, local_ms*1000ULL) / 1000ULL;
}

/*------------------------------------test------------------------------------*/


